﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: n/a
// Revision   : n/a
// Change-Date: n/a
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2CollideEdge.cpp
// Revision   : r132
// Change-Date: 2010-09-21
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public enum EdgeType
    {
        ISOLATED,
        CONCAVE,
        FLAT,
        CONVEX,
    }

    public partial class Collision
    {
        // Compute contact points for edge versus circle.
        // This accounts for edge connectivity.
        internal static void CollideEdgeAndCircle(out Manifold manifold, EdgeShape edgeA, ref Transform xfA,
                                                                         CircleShape circleB, ref Transform xfB)
        {
#if !OPTIMIZE
            manifold = new Manifold();
#else
            manifold = ManifoldPool.Instance.Allocate();
#endif

            manifold.pointCount = 0;

            // Compute circle in frame of edge
            Vector2 Q = Math.MulT(xfA, Math.Mul(xfB, circleB.m_p));

            Vector2 A = edgeA.m_vertex1, B = edgeA.m_vertex2;
            Vector2 e = B - A;

            // Barycentric coordinates
            float u = Vector2.Dot(e, B - Q);
            float v = Vector2.Dot(e, Q - A);

            float radius = edgeA.m_radius + circleB.m_radius;

            ContactFeature cf = new ContactFeature();
            cf.indexB = 0;
            cf.typeB = ContactFeature.Type.VERTEX;

            // Region A
            if (v <= 0.0f)
            {
                Vector2 P = A;
                Vector2 d = Q - P;
                float dd = Vector2.Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to A?
                if (edgeA.m_hasVertex0)
                {
                    Vector2 A1 = edgeA.m_vertex0;
                    Vector2 B1 = A;
                    Vector2 e1 = B1 - A1;
                    float u1 = Vector2.Dot(e1, B1 - Q);

                    // Is the circle in Region AB of the previous edge?
                    if (u1 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 0;
                cf.typeA = ContactFeature.Type.VERTEX;
                manifold.pointCount = 1;
                manifold.type = ManifoldType.e_circles;
                manifold.localNormal = Vector2.Zero;
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.cf = cf;
                manifold.points[0].localPoint = circleB.m_p;
                return;
            }

            // Region B
            if (u <= 0.0f)
            {
                Vector2 P = B;
                Vector2 d = Q - P;
                float dd = Vector2.Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                // Is there an edge connected to B?
                if (edgeA.m_hasVertex3)
                {
                    Vector2 B2 = edgeA.m_vertex3;
                    Vector2 A2 = B;
                    Vector2 e2 = B2 - A2;
                    float v2 = Vector2.Dot(e2, Q - A2);

                    // Is the circle in Region AB of the next edge?
                    if (v2 > 0.0f)
                    {
                        return;
                    }
                }

                cf.indexA = 1;
                cf.typeA = ContactFeature.Type.VERTEX;
                manifold.pointCount = 1;
                manifold.type = ManifoldType.e_circles;
                manifold.localNormal = Vector2.Zero;
                manifold.localPoint = P;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.cf = cf;
                manifold.points[0].localPoint = circleB.m_p;
                return;
            }

            // Region AB
            {
                float den = Vector2.Dot(e, e);
                System.Diagnostics.Debug.Assert(den > 0.0f);
                Vector2 P = (1.0f / den) * (u * A + v * B);
                Vector2 d = Q - P;
                float dd = Vector2.Dot(d, d);
                if (dd > radius * radius)
                {
                    return;
                }

                Vector2 n = new Vector2(-e.Y, e.X);
                if (Vector2.Dot(n, Q - A) < 0.0f)
                {
                    n.X = -n.X; n.Y = -n.Y;
                }
                n.Normalize();

                cf.indexA = 0;
                cf.typeA = ContactFeature.Type.FACE;
                manifold.pointCount = 1;
                manifold.type = ManifoldType.e_faceA;
                manifold.localNormal = n;
                manifold.localPoint = A;
                manifold.points[0].id.key = 0;
                manifold.points[0].id.cf = cf;
                manifold.points[0].localPoint = circleB.m_p;
            }
        }

        public struct EPAxis
        {
            public enum Type
            {
                UNKNOWN,
                EDGE_A,
                EDGE_B,
            }

            public EPAxis.Type type;
            public int index;
            public float separation;
        }

// Edge shape plus more stuff.
        public struct FatEdge
        {
            public Vector2 v0, v1, v2, v3;
            public Vector2 normal;
            public bool hasVertex0, hasVertex3;
        }

        // This lets us treate and edge shape and a polygon in the same
        // way in the SAT collider.
        public struct EPProxy
        {
            public Vector2[] vertices;
            public Vector2[] normals;
            public Vector2 centroid;
            public int count;
        }

        // This class collides an edge and a polygon, taking into account edge adjacency.
        public class EPCollider
        {
            public EPCollider(EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
            {
                m_xf = Math.MulT(xfA, xfB);

                // Edge geometry
                m_edgeA.v0 = edgeA.m_vertex0;
                m_edgeA.v1 = edgeA.m_vertex1;
                m_edgeA.v2 = edgeA.m_vertex2;
                m_edgeA.v3 = edgeA.m_vertex3;
                Vector2 e = m_edgeA.v2 - m_edgeA.v1;

                // Normal points outwards in CCW order.
                m_edgeA.normal = new Vector2(e.Y, -e.X);
                m_edgeA.normal.Normalize();
                m_edgeA.hasVertex0 = edgeA.m_hasVertex0;
                m_edgeA.hasVertex3 = edgeA.m_hasVertex3;

                // Proxy for edge
                m_proxyA.vertices = new Vector2[2];
                m_proxyA.vertices[0] = m_edgeA.v1;
                m_proxyA.vertices[1] = m_edgeA.v2;
                m_proxyA.normals = new Vector2[2];
                m_proxyA.normals[0] = m_edgeA.normal;
                m_proxyA.normals[1] = -m_edgeA.normal;
                m_proxyA.centroid = 0.5f * (m_edgeA.v1 + m_edgeA.v2);
                m_proxyA.count = 2;

                // Proxy for polygon
                m_proxyB.count = polygonB.m_vertexCount;
                m_proxyB.centroid = Math.Mul(m_xf, polygonB.m_centroid);
                m_proxyB.vertices = new Vector2[m_proxyB.count];
                m_proxyB.normals = new Vector2[m_proxyB.count];
                for (int i = 0; i < polygonB.m_vertexCount; ++i)
                {
                    m_proxyB.vertices[i] = Math.Mul(m_xf, polygonB.m_vertices[i]);
                    m_proxyB.normals[i] = Math.Mul(m_xf.R, polygonB.m_normals[i]);
                }

                m_radius = 2.0f * PhysicSettings.PolygonRadius;

                m_limit11 = Vector2.Zero;
                m_limit12 = Vector2.Zero;
                m_limit21 = Vector2.Zero;
                m_limit22 = Vector2.Zero;
            }

            // Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
            // Edge adjacency is handle to produce locally valid contact points and normals. This is intended
            // to allow the polygon to slide smoothly over an edge chain.
            //
            // Algorithm
            // 1. Classify front-side or back-side collision with edge.
            // 2. Compute separation
            // 3. Process adjacent edges
            // 4. Classify adjacent edge as convex, flat, null, or concave
            // 5. Skip null or concave edges. Concave edges get a separate manifold.
            // 6. If the edge is flat, compute contact points as normal. Discard boundary points.
            // 7. If the edge is convex, compute it's separation.
            // 8. Use the minimum separation of up to three edges. If the minimum separation
            //    is not the primary edge, return.
            // 9. If the minimum separation is the primary edge, compute the contact points and return.
            public void Collide(out Manifold manifold)
            {
                manifold = new Manifold();
                manifold.pointCount = 0;

                ComputeAdjacency();

                EPAxis edgeAxis = ComputeEdgeSeparation();

                // If no valid normal can be found than this edge should not collide.
                // This can happen on the middle edge of a 3-edge zig-zag chain.
                if (edgeAxis.type == EPAxis.Type.UNKNOWN)
                {
                    return;
                }

                if (edgeAxis.separation > m_radius)
                {
                    return;
                }

                EPAxis polygonAxis = ComputePolygonSeparation();
                if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius)
                {
                    return;
                }

                // Use hysteresis for jitter reduction.
                const float k_relativeTol = 0.98f;
                const float k_absoluteTol = 0.001f;

                EPAxis primaryAxis;
                if (polygonAxis.type == EPAxis.Type.UNKNOWN)
                {
                    primaryAxis = edgeAxis;
                }
                else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
                {
                    primaryAxis = polygonAxis;
                }
                else
                {
                    primaryAxis = edgeAxis;
                }

                EPProxy proxy1;
                EPProxy proxy2;
                ClipVertex[] incidentEdge = new ClipVertex[2];
                if (primaryAxis.type == EPAxis.Type.EDGE_A)
                {
                    proxy1 = m_proxyA;
                    proxy2 = m_proxyB;
                    manifold.type = ManifoldType.e_faceA;
                }
                else
                {
                    proxy1 = m_proxyB;
                    proxy2 = m_proxyA;
                    manifold.type = ManifoldType.e_faceB;
                }

                int edge1 = primaryAxis.index;

                FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);
                int count1 = proxy1.count;
                Vector2[] vertices1 = proxy1.vertices;

                int iv1 = edge1;
                int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;

                Vector2 v11 = vertices1[iv1];
                Vector2 v12 = vertices1[iv2];

                Vector2 tangent = v12 - v11;
                tangent.Normalize();

                Vector2 normal = Vector2Extensions.Cross(tangent, 1.0f);
                Vector2 planePoint = 0.5f * (v11 + v12);

                // Face offset.
                float frontOffset = Vector2.Dot(normal, v11);

                // Side offsets, extended by polytope skin thickness.
                float sideOffset1 = -Vector2.Dot(tangent, v11) + m_radius;
                float sideOffset2 = Vector2.Dot(tangent, v12) + m_radius;

                // Clip incident edge against extruded edge1 side edges.
                ClipVertex[] clipPoints1 = new ClipVertex[2];
                ClipVertex[] clipPoints2 = new ClipVertex[2];
                int np;

                // Clip to box side 1
                Vector2 nTangent = -tangent;
                np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, ref nTangent, ref sideOffset1, ref iv1);

                if (np < PhysicSettings.MaxManifoldPoints)
                {
                    return;
                }

                // Clip to negative box side 1
                np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref tangent, ref sideOffset2, ref iv2);

                if (np < PhysicSettings.MaxManifoldPoints)
                {
                    return;
                }

                // Now clipPoints2 contains the clipped points.
                if (primaryAxis.type == EPAxis.Type.EDGE_A)
                {
                    manifold.localNormal = normal;
                    manifold.localPoint = planePoint;
                }
                else
                {
                    manifold.localNormal = Math.MulT(m_xf.R, normal);
                    manifold.localPoint = Math.MulT(m_xf, planePoint);
                }

                int pointCount = 0;
                for (int i = 0; i < PhysicSettings.MaxManifoldPoints; ++i)
                {
                    float separation;

                    separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;

                    if (separation <= m_radius)
                    {
                        ManifoldPoint cp = manifold.points[pointCount];

                        if (primaryAxis.type == EPAxis.Type.EDGE_A)
                        {
                            cp.localPoint = Math.MulT(m_xf, clipPoints2[i].v);
                            cp.id = clipPoints2[i].id;
                        }
                        else
                        {
                            cp.localPoint = clipPoints2[i].v;
                            cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
                            cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
                            cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
                            cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
                        }

                        manifold.points[pointCount] = cp;
                        ++pointCount;
                    }
                }

                manifold.pointCount = pointCount;
            }

            // Compute allowable normal ranges based on adjacency.
            // A normal n is allowable iff:
            // cross(n, n1) >= 0.0f && cross(n2, n) >= 0.0f
            // n points from A to B (edge to polygon)
            public void ComputeAdjacency()
            {
                Vector2 v0 = m_edgeA.v0;
                Vector2 v1 = m_edgeA.v1;
                Vector2 v2 = m_edgeA.v2;
                Vector2 v3 = m_edgeA.v3;

                // Determine allowable the normal regions based on adjacency.
                // Note: it may be possible that no normal is admissable.
                Vector2 centerB = m_proxyB.centroid;
                if (m_edgeA.hasVertex0)
                {
                    Vector2 e0 = v1 - v0;
                    Vector2 e1 = v2 - v1;
                    Vector2 n0 = new Vector2(e0.Y, -e0.X);
                    Vector2 n1 = new Vector2(e1.Y, -e1.X);
                    n0.Normalize();
                    n1.Normalize();

                    bool convex = Vector2Extensions.Cross(n0, n1) >= 0.0f;
                    bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
                    bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;

                    if (convex)
                    {
                        if (front0 || front1)
                        {
                            m_limit11 = n1;
                            m_limit12 = n0;
                        }
                        else
                        {
                            m_limit11 = -n1;
                            m_limit12 = -n0;
                        }
                    }
                    else
                    {
                        if (front0 && front1)
                        {
                            m_limit11 = n0;
                            m_limit12 = n1;
                        }
                        else
                        {
                            m_limit11 = -n0;
                            m_limit12 = -n1;
                        }
                    }
                }
                else
                {
                    m_limit11 = Vector2.Zero;
                    m_limit12 = Vector2.Zero;
                }

                if (m_edgeA.hasVertex3)
                {
                    Vector2 e1 = v2 - v1;
                    Vector2 e2 = v3 - v2;
                    Vector2 n1 = new Vector2(e1.Y, -e1.X);
                    Vector2 n2 = new Vector2(e2.Y, -e2.X);
                    n1.Normalize();
                    n2.Normalize();

                    bool convex = Vector2Extensions.Cross(n1, n2) >= 0.0f;
                    bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
                    bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;

                    if (convex)
                    {
                        if (front1 || front2)
                        {
                            m_limit21 = n2;
                            m_limit22 = n1;
                        }
                        else
                        {
                            m_limit21 = -n2;
                            m_limit22 = -n1;
                        }
                    }
                    else
                    {
                        if (front1 && front2)
                        {
                            m_limit21 = n1;
                            m_limit22 = n2;
                        }
                        else
                        {
                            m_limit21 = -n1;
                            m_limit22 = -n2;
                        }
                    }
                }
                else
                {
                    m_limit21 = Vector2.Zero;
                    m_limit22 = Vector2.Zero;
                }
            }

            public EPAxis ComputeEdgeSeparation()
            {
                // EdgeA separation
                EPAxis bestAxis;
                bestAxis.type = EPAxis.Type.UNKNOWN;
                bestAxis.index = -1;
                bestAxis.separation = -PhysicSettings.FLT_MAX;
                Vector2[] normals = new Vector2[] {m_edgeA.normal, -m_edgeA.normal};

                for (int i = 0; i < 2; ++i)
                {
                    Vector2 n = normals[i];

                    // Adjacency
                    bool valid1 = Vector2Extensions.Cross(n, m_limit11) >= -PhysicSettings.AngularSlop && Vector2Extensions.Cross(m_limit12, n) >= -PhysicSettings.AngularSlop;
                    bool valid2 = Vector2Extensions.Cross(n, m_limit21) >= -PhysicSettings.AngularSlop && Vector2Extensions.Cross(m_limit22, n) >= -PhysicSettings.AngularSlop;

                    if (valid1 == false || valid2 == false)
                    {
                        continue;
                    }

                    EPAxis axis;
                    axis.type = EPAxis.Type.EDGE_A;
                    axis.index = i;
                    axis.separation = PhysicSettings.FLT_MAX;

                    for (int j = 0; j < m_proxyB.count; ++j)
                    {
                        float s = Vector2.Dot(n, m_proxyB.vertices[j] - m_edgeA.v1);
                        if (s < axis.separation)
                        {
                            axis.separation = s;
                        }
                    }

                    if (axis.separation > m_radius)
                    {
                        return axis;
                    }

                    if (axis.separation > bestAxis.separation)
                    {
                        bestAxis = axis;
                    }
                }

                return bestAxis;
            }

            public EPAxis ComputePolygonSeparation()
            {
                EPAxis axis;
                axis.type = EPAxis.Type.UNKNOWN;
                axis.index = -1;
                axis.separation = -PhysicSettings.FLT_MAX;
                for (int i = 0; i < m_proxyB.count; ++i)
                {
                    Vector2 n = -m_proxyB.normals[i];

                    // Adjacency
                    bool valid1 = Vector2Extensions.Cross(n, m_limit11) >= -PhysicSettings.AngularSlop && Vector2Extensions.Cross(m_limit12, n) >= -PhysicSettings.AngularSlop;
                    bool valid2 = Vector2Extensions.Cross(n, m_limit21) >= -PhysicSettings.AngularSlop && Vector2Extensions.Cross(m_limit22, n) >= -PhysicSettings.AngularSlop;

                    if (valid1 == false && valid2 == false)
                    {
                        continue;
                    }

                    float s1 = Vector2.Dot(n, m_proxyB.vertices[i] - m_edgeA.v1);
                    float s2 = Vector2.Dot(n, m_proxyB.vertices[i] - m_edgeA.v2);
                    float s = MathHelper.Min(s1, s2);

                    if (s > m_radius)
                    {
                        axis.type = EPAxis.Type.EDGE_B;
                        axis.index = i;
                        axis.separation = s;
                    }

                    if (s > axis.separation)
                    {
                        axis.type = EPAxis.Type.EDGE_B;
                        axis.index = i;
                        axis.separation = s;
                    }
                }

                return axis;
            }

            public void FindIncidentEdge(ClipVertex[] c, EPProxy proxy1, int edge1, EPProxy proxy2)
            {
                int count1 = proxy1.count;
                Vector2[] normals1 = proxy1.normals;

                int count2 = proxy2.count;
                Vector2[] vertices2 = proxy2.vertices;
                Vector2[] normals2 = proxy2.normals;

                System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < count1);

                // Get the normal of the reference edge in proxy2's frame.
                Vector2 normal1 = normals1[edge1];

                // Find the incident edge on proxy2.
                int index = 0;
                float minDot = float.MaxValue;
                for (int i = 0; i < count2; ++i)
                {
                    float dot = Vector2.Dot(normal1, normals2[i]);
                    if (dot < minDot)
                    {
                        minDot = dot;
                        index = i;
                    }
                }
    
                // Build the clip vertices for the incident edge.
                int i1 = index;
                int i2 = i1 + 1 < count2 ? i1 + 1 : 0;

                c[0].v = vertices2[i1];
                c[0].id.cf.indexA = (byte)edge1;
                c[0].id.cf.indexB = (byte)i1;
                c[0].id.cf.typeA = ContactFeature.Type.FACE;
                c[0].id.cf.typeB = ContactFeature.Type.VERTEX;

                c[1].v = vertices2[i2];
                c[1].id.cf.indexA = (byte)edge1;
                c[1].id.cf.indexB = (byte)i2;
                c[1].id.cf.typeA = ContactFeature.Type.FACE;
                c[1].id.cf.typeB = ContactFeature.Type.VERTEX;
            }

            public FatEdge m_edgeA;
            public EPProxy m_proxyA;
            public EPProxy m_proxyB;
            public Transform m_xf;
            public Vector2 m_normal0;
            public Vector2 m_normal2;
            public Vector2 m_limit11;
            public Vector2 m_limit12;
            public Vector2 m_limit21;
            public Vector2 m_limit22;
            public float m_radius;
        }

        public static void CollideEdgeAndPolygon(out Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
        {
            EPCollider collider = new EPCollider(edgeA, ref xfA, polygonB, ref xfB);
            collider.Collide(out manifold);
        }

    }
}
